#include "motion.h"
#include "math.h"
struct motion_info motion = {
    .max_speed = 100,
    .position = {
        .x = 0,
        .y = 0,
        .z = 0},
    .stepper_x = &motor_x,
    .stepper_y = &motor_y,
    .velocity = {.x = 0, .y = 0, .z = 0}};

static void v3_diff(struct vector3 *dest, struct vector3 *src, struct vector3 *res)
{
    res->x = dest->x - src->x;
    res->y = dest->y - src->y;
    res->z = dest->z - src->z;
}

static void v3_copy(struct vector3 *dest, struct vector3 *src)
{
    dest->x = src->x;
    dest->y = src->y;
    dest->z = src->z;
}

void motion_goto(struct motion_info *ins, struct vector3 *dest)
{
    while (ins->motion_lock == 1)
        ;
    ins->motion_lock = 1;
    struct vector3 v3DisplaceXY;
    struct vector3 v3DisplaceMotor;
    struct vector3 v3SpeedScaled;
    v3_diff(dest, &ins->position, &v3DisplaceXY);
    v3_copy(&ins->position, dest);

#ifdef COREXY
#endif
    v3DisplaceMotor.x = v3DisplaceXY.x + v3DisplaceXY.y;
    v3DisplaceMotor.y = v3DisplaceXY.x - v3DisplaceXY.y;
#ifdef XY
    v3DisplaceMotor.x = v3DisplaceXY.x;
    v3DisplaceMotor.y = v3DisplaceXY.y;
#endif

    int32_t vscale = STEP_PER_MM * sqrt(v3DisplaceMotor.x * v3DisplaceMotor.x + v3DisplaceMotor.y * v3DisplaceMotor.y);

    if (vscale == 0)
    {
        v3SpeedScaled.x = 0;
        v3SpeedScaled.y = 0;
        ins->motion_lock = 0;
        return;
    }
    else
    {
        // unit: mm/s -> steps/s
        v3SpeedScaled.x = v3DisplaceMotor.x * ins->max_speed * STEP_PER_MM * STEP_PER_MM / vscale;
        v3SpeedScaled.y = v3DisplaceMotor.y * ins->max_speed * STEP_PER_MM * STEP_PER_MM / vscale;
    }
    v3_copy(&ins->velocity, &v3SpeedScaled);
    ins->stepper_x->motor_lock = 1;
    ins->stepper_y->motor_lock = 1;
    if (((int32_t)(v3SpeedScaled.x)) != 0)
        stepper_launch(ins->stepper_x, (int32_t)v3SpeedScaled.x, 1000000 * STEP_PER_MM * v3DisplaceMotor.x / v3SpeedScaled.x);
    else
        stepper_launch(ins->stepper_x, 0, 0);

    if (((int32_t)(v3SpeedScaled.y)) != 0)
        stepper_launch(ins->stepper_y, (int32_t)v3SpeedScaled.y, 1000000 * STEP_PER_MM * v3DisplaceMotor.y / v3SpeedScaled.y);
    else
        stepper_launch(ins->stepper_y, 0, 0);

    while (ins->stepper_x->motor_lock == 1 || ins->stepper_y->motor_lock == 1)
        ;
    ins->motion_lock = 0;
}
